#include "controller.h"
#include "nxbot.h"
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <avr/io.h>
#include "lcd.h"

void PI_init(parametersPI * pi){
	pi->right_acumm_error=0;
	pi->left_acumm_error=0;
	
	pi->Kp=((float)(pi->Kp_int))/100;
	pi->Ki=((float)(pi->Ki_int))/100;
	
	setVelMotor((signed char)0,MOTOR_RIGHT);
	setVelMotor((signed char)0,MOTOR_LEFT);
}

int PIController(parametersPI * pi, velocities * vels){
	
	static int16_t pwm_right=0;
	static int16_t pwm_left=0;
	
	volatile float kp=pi->Kp;
	volatile float ki=pi->Ki;
	
	//int32_t max_acumm_error=MAX_INTEGRAL_TERM/(ki+1);
		
	int16_t proportional_right,proportional_left;
	
	int32_t tempr, templ, integral_right,integral_left;

	//Calculo del error
	////////////////////////////////////////////////////////
	int16_t right_error=(vels->velRightRef)-(vels->velRightAct);
	int16_t left_error=(vels->velLeftRef)-(vels->velLeftAct);
	////////////////////////////////////////////////////////
	
	// calculo de los terminos proporcionales para cada motor
	///////////////////////////////////////////////////////////
	proportional_right=(int16_t)(kp*((float)(right_error)));
	///////////////////////////////////////////////////////////
	proportional_left=(int16_t)(kp*((float)(left_error)));
	///////////////////////////////////////////////////////////

	// Calculo de los terminos integrales
	///////////////////////////////////////////////////////////
	tempr=pi->right_acumm_error+(int32_t)(right_error>>2);
	templ=pi->left_acumm_error+(int32_t)(left_error>>2);
	//////////////////////////////////////////////////////////
	if(tempr>MAX_INTEGRAL_TERM){
		integral_right=MAX_INTEGRAL_TERM;
		pi->right_acumm_error=MAX_INTEGRAL_TERM;
	}
	else if(tempr<-MAX_INTEGRAL_TERM){
		integral_right=-MAX_INTEGRAL_TERM;
		pi->right_acumm_error=-MAX_INTEGRAL_TERM;		
	}	
	else{
		pi->right_acumm_error=tempr;
		integral_right=(int32_t)(ki*((float)(pi->right_acumm_error)));
	} 
	///////////////////////////////////////////////////////////
	if(templ>MAX_INTEGRAL_TERM){
		integral_left=MAX_INTEGRAL_TERM;
		pi->left_acumm_error=MAX_INTEGRAL_TERM;
	}
	else if(templ<-MAX_INTEGRAL_TERM){
		integral_left=-MAX_INTEGRAL_TERM;
		pi->left_acumm_error=-MAX_INTEGRAL_TERM;		
	}	
	else{
		pi->left_acumm_error=templ;
		integral_left=(int32_t)(ki*((float)(pi->left_acumm_error)));
	}
	////////////////////////////////////////////////////////////
	int32_t output_pi_right=proportional_right;
	output_pi_right+=integral_right;
	
	int32_t output_pi_left=proportional_left;
	output_pi_left+=integral_left;
	
	pwm_right+= output_pi_right>>1;
	pwm_left+= output_pi_left>>1;  
	
	pwm_right=bound(pwm_right,-127,127);
	pwm_left=bound(pwm_left,-127,127);
	
	setVelMotor((signed char)pwm_right,MOTOR_RIGHT);
	setVelMotor((signed char)pwm_left,MOTOR_LEFT);
	
	return 0;
}

INLINE_FUN int16_t bound(int16_t value,int16_t min,int16_t max){
	if(value>max) return max;
	else if(value<min) return min;
	else return value;	
}


/*void PIController2(parametersPI * pi, velocities * vels){
	static int16_t pwm_right=0;
	static int16_t pwm_left=0;
	
	float kp=((float)(pi->Kp))/100;
	float ki=((float)(pi->Ki))/100;
	
	int16_t integral;
	
	//int32_t max_acumm_error=MAX_INTEGRAL_TERM/(ki+1);
		
	int16_t bias_ref=((vels->velRightRef)*MAX_BIAS)/100;
	int16_t vel_ref=((vels->velLeftRef)*MAX_VEL)/100;
	
	int16_t vel_right=vels->velRightAct;
	int16_t vel_left=vels->velLeftAct;
	
	pi->right_acumm_error+=((vel_left+bias_ref-vel_right));
	
	integral=(int16_t)(ki*((float)(pi->right_acumm_error)));
	
	int16_t right_error=((vel_ref-vel_right))+integral;
	int16_t left_error=((vel_ref-vel_left))-integral;
	
	int16_t output_right=(int16_t)(kp*((float)right_error));
	int16_t output_left=(int16_t)(kp*((float)left_error));
	
	pwm_right+= (output_right*127)/255;
	pwm_left+= (output_left*127)/255;  
	
	if(pwm_right>127)
		pwm_right=127;
	else if(pwm_right<-127)
		pwm_right=-127;
	
	if(pwm_left>127)
		pwm_left=127;
	else if(pwm_left<-127)
		pwm_left=-127;
	
	setVelMotor((signed char)pwm_right,MOTOR_RIGHT);
	setVelMotor((signed char)pwm_left,MOTOR_LEFT);
}
*/
/*int PController(float kp,int x,unsigned char motor)
{

//if(motor)
	
	
	return 0;
}



int IController(float ki,unsigned int x)
{
	return 0;
}
*/


